#include "ros/ros.h"

#include <cstdlib>
#include "camControl/visualization.hpp"
#include "camControl/algorithmImage.hpp"
#include "camControl/camera.h"
#include "camControl/robot.hpp"
#include "camControl/setRobot.hpp"

#include "camControl/CameraControlMsg.h"

int main(int argc, char** argv) {

    ros::init(argc,argv,"cameraControl");
    ros::NodeHandle n;
    ros::Publisher robotMsg;
    
    camControl::CameraControlMsg camMsg;
    robotMsg = n.advertise<camControl::CameraControlMsg>("controlEpuck",1);

#ifdef OPENCV_USE_CAMERA

    CvCapture *capture = cvCaptureFromCAM(CV_CAP_ANY);
    IplImage *frame = cvQueryFrame(capture);
    AlgorithmImage aImage(frame->width, frame->height);

#else
    FlyCapture2::Camera cam;
    FlyCapture2::Error error;

    camOpenConfig(&cam, &error);

    FlyCapture2::Image RGB;
    FlyCapture2::Image Bayer;

    CaptureImage(&RGB, &Bayer, &cam, &error);
    AlgorithmImage aImage(RGB.GetCols(), RGB.GetRows());

#endif

    SetRobot setRobot;
    setRobot.setupKalmanFilterRobots();

    int DM = 4;
    int MM = 2;
    int CM = 0;

    float deltaTime = 0.005;
    float qX = 0.05;
    float qY = 0.05;
    //    float qTheta = 0.05;
    float rX = 0.1;
    float rY = 0.1;
    //    float rTheta = 0.1;

    float Q_[] = {
        qX, 0, 0, 0,
        0, qY, 0, 0,
        0, 0, qX, 0,
        0, 0, 0, qX
    };

    float R_[] = {
        rX, 0,
        0, rY
    };

    float A_[] = {
        1, 0, deltaTime, 0,
        0, 1, 0, deltaTime,
        0, 0, 1, 0,
        0, 0, 0, 1
    };

    float H_[] = {
        1, 0, 0, 0,
        0, 1, 0, 0
    };

    float p_[] = {
        1, 0, 0, 0,
        0, 1, 0, 0,
        0, 0, 1, 0,
        0, 0, 0, 1
    };

    float x_[] = {0, 0, 1, 1};

    Kalman<float> *setKalman[NUM_MAX_ROBOTS];

    for (int i = 0; i < NUM_MAX_ROBOTS; i++)
        setKalman[i] = new Kalman<float>(DM, MM, CM, A_, NULL, H_, Q_, R_, p_, x_);

    INFO_ROBOT info;
    bool exec = true;
    ros::Rate loopRate(100);    

    while (exec) {


#ifdef OPENCV_USE_CAMERA  
        frame = cvQueryFrame(capture);
        aImage.setImageOpenCV(frame->imageData);
#else
        CaptureImage(&RGB, &Bayer, &cam, &error);
        aImage.setImageOpenCV(RGB.GetData());
#endif   
        aImage.segmentedImage();
        aImage.findConnectedComponents();

        setRobot.findRobotsPose2D(aImage.getNumRec(), aImage.getColors(), aImage.getCenters());

        for (int i = 0; i < NUM_MAX_ROBOTS; i++) {

            info = setRobot.getInfoRobotNUM(i);
            //            kalmanF.updateKalman(info);
            setKalman[i]->updateKalman(info);
            setRobot.setInfoRobotNum(i, info);
            //            setRobot.setSetRobotPosition(i,info);

//            camMsg.find[i] = info.find;
//            camMsg.pose2d[i].x = info.x;
//            camMsg.pose2d[i].y = info.y;
//            camMsg.pose2d[i].theta = info.theta;        
        }

        
        cvShowImage("Image", aImage.getImageOpenCV());
        cvShowImage("Image Kalman", setRobot.showKalmanFilterAction());


 //        robotMsg.publish(camMsg);        

        if ((cvWaitKey(10) & 255) == 27) exec = false;
        else loopRate.sleep();

        ros::spinOnce();
    }

#ifndef OPENCV_USE_CAMERA
    camCleanUp(&cam, error);
#else
    cvReleaseCapture(&capture);
#endif

    //    for(int i=0;i < NUM_MAX_ROBOTS;i++)
    //        delete[] setKalman[i];
    return EXIT_SUCCESS;
}

